#include "command.h"
extern command cOut,C;

/* Constant */
#define BLINK_FREQ 1000
#define BLINK_PERI 300

#define WAIT_DLY 100

#define iProg_idxPosY 0
const int iProg_idxPosX[6] = {6,7,8,10,11,12};

#define iProg_msgPosY 1
const int iProg_msgPosX[2] = {2, 9};
#define iProgClearMSG   "            "
#define iProgIDSetMSG   "SET ID"
#define iProgIDGetMSG   "GET ID"
#define iProgFailMSG    "FAIL!"
#define iProgOKMSG      "OK!  "

int setAbs, setOrd, getAbs, getOrd;
char iProg_XY[6];
int iProg_cur;
int iProg_enableBlink;

void idProg_CreateMenu() {
    lcd.setCursor(0,0);
    lcd.print("* THERMALPLATE *");
    lcd.setCursor(0,1);
    lcd.print("** ID SETTER  **");
    delay(1000);
    
    
    
    lcd.setCursor(0,0);
    lcd.print("ID: ( 000,000 ) ");
    lcd.setCursor(0,1);
       //      0123456789012345
    lcd.print("                ");
    
    iProg_cur = 0;  
    idProg_updateAbsisOrdinatInt(0,0);
    
    iProg_enableBlink = 1;
}

void idProg_updateAbsisOrdinat (){
    for (int i = 0; i < 6; ++i){
        lcd.setCursor(iProg_idxPosX[i] ,iProg_idxPosY );
        lcd.write(iProg_XY[i]);
    }
}

void idProg_updateAbsisOrdinatInt (int _absis, int _ordinat){
    iProg_XY[0] = '0' + (int) (_absis / 100);
    iProg_XY[1] = '0' + (int) ( (int) (_absis % 100 )/ 10);
    iProg_XY[2] = '0' + (_absis % 10);
    iProg_XY[3] = '0' + (int) (_ordinat / 100);
    iProg_XY[4] = '0' + (int)( (int) (_ordinat % 100 )/ 10);
    iProg_XY[5] = '0' + (_ordinat % 10);
    
    
    idProg_updateAbsisOrdinat();
}

void idProg_blinkCur() {
    if (iProg_enableBlink){
    if (millis() % BLINK_FREQ < BLINK_PERI) {
        lcd.setCursor(iProg_idxPosX[iProg_cur] ,iProg_idxPosY );
        lcd.write('_');
    } else {
        lcd.setCursor(iProg_idxPosX[iProg_cur] ,iProg_idxPosY );
        lcd.write(iProg_XY[iProg_cur]);
    }} else {
        idProg_updateAbsisOrdinat();
    }
}





int idProg_loop() {
    char key = getKey();
  
    idProg_blinkCur();
    if (key != KP_EMPTY ){
        #ifdef DEBUG_SER
        Serial.print("Keypad : ");
        Serial.println(key);
        #endif
    
        if (key == '*') {
            iProg_enableBlink = 0;
            //GET ID
            lcd.setCursor(iProg_msgPosX[0],iProg_msgPosY);
            lcd.print(iProgIDGetMSG);
            lcd.setCursor(iProg_msgPosX[1],iProg_msgPosY);
            lcd.print(iProgFailMSG);
            
            setAbs = 0;                       
            setOrd = 0; 
            iProg_cur = 0;
            idProg_setIDProc();
            
        } else if (key == '#') {

            //SET ID
            
            iProg_enableBlink = 0;
            lcd.setCursor(iProg_msgPosX[0],iProg_msgPosY);
            lcd.print(iProgIDSetMSG);
            lcd.setCursor(iProg_msgPosX[1],iProg_msgPosY);
            lcd.print(iProgFailMSG);
            
            setAbs = (iProg_XY[0]-'0')*100 + 
                   (iProg_XY[1]-'0')*10 +
                   (iProg_XY[2]-'0');
                       
            setOrd = (iProg_XY[3]-'0')*100 + 
                   (iProg_XY[4]-'0')*10 +
                   (iProg_XY[5]-'0');
            
            #ifdef DEBUG_SER
            Serial.print("SET ( ");
            Serial.print(setAbs);
            Serial.print(" , ");
            Serial.print(setOrd);
            Serial.println(" )");            
            #endif        

            
            iProg_cur = 0; 
            idProg_setIDProc();
            /*if (idProg_getIDProc()) {
                lcd.print("SET OK ");
            }*/
            
           
        } else if ((key >= 'A') && (key <= 'D')) {

            iProg_enableBlink = 1;
            lcd.setCursor(iProg_msgPosX[0],iProg_msgPosY);
            lcd.print(iProgClearMSG);
            
            setAbs = (iProg_XY[0]-'0')*100 + 
                   (iProg_XY[1]-'0')*10 +
                   (iProg_XY[2]-'0');
                       
            setOrd = (iProg_XY[3]-'0')*100 + 
                   (iProg_XY[4]-'0')*10 +
                   (iProg_XY[5]-'0');
            
            switch(key){
                case 'A':
                    setAbs = (setAbs==999)?999:setAbs+1;
                break;
                case 'B':
                    setAbs = (setAbs>0)?setAbs-1:0;
                break;
                case 'C':
                    setOrd = (setOrd>999)?999:setOrd+1;
                break;
                case 'D':
                    setOrd = (setOrd>0)?setOrd-1:0;
                break;
                
            }
            
            iProg_cur = 0; 
            idProg_updateAbsisOrdinatInt(setAbs, setOrd);
            /*if (idProg_getIDProc()) {
                lcd.print("SET OK ");
            }*/
            
           
        } else if ((key >= '0') && (key <= '9')) {
            // INPUT 
            iProg_enableBlink = 1;
            lcd.setCursor(iProg_msgPosX[0],iProg_msgPosY);
            lcd.print(iProgClearMSG);
            
            iProg_XY[iProg_cur] = key;
            iProg_cur = (iProg_cur==5)?0:(iProg_cur+1);
            idProg_updateAbsisOrdinat();
        }
    }
    return 0;
}


void idProg_setIDProc(){
    getAbs = 0;                       
    getOrd = 0;
                       
    cOut = makeCommand(SETID_CMD,  setAbs, setOrd, 0);   
    sendCOut();
    
    unsigned long long start_time = millis();
    while(start_time+WAIT_DLY > millis()) {
        processSerial();
    }
    
    if ((((setAbs == 0) && (setOrd == 0))||
       ((setAbs == getAbs) && (setOrd == getOrd)))
       && ((getOrd != 0) && (getAbs != 0))){
        
        idProg_updateAbsisOrdinatInt(getAbs, getOrd);
        lcd.setCursor(iProg_msgPosX[1],iProg_msgPosY);
        lcd.print(iProgOKMSG);
    
    }
}

